//
// Created by cyq on 2022/12/5.
//

#ifndef DRIVER_CHASSIS_H
#define DRIVER_CHASSIS_H

#include "main.h"
#include "driver_motor.h"
#include "driver_superc.h"
#include "task_gyro.h"
#include "ramp.h"



#define CHASSISMOTOR_SPEED_KP (5.0f)
#define CHASSISMOTOR_SPEED_KI (0.0f)
#define CHASSISMOTOR_SPEED_KD (0.5f)
#define CHASSISMOTOR_SPEED_UIOUTMAX (0.01f)
#define CHASSISMOTOR_SPEED_OUTMAX (1.0f)

#define CHASSISMOTOR_LOCTION_KP (0.6f)
#define CHASSISMOTOR_LOCTION_KI (0.0f)
#define CHASSISMOTOR_LOCTION_KD (0.1f)
#define CHASSISMOTOR_LOCTION_UIOUTMAX (0.3f)
#define CHASSISMOTOR_LOCTION_OUTMAX (1.0f)


#define CHASSISMOTOR_FOLLOW_ANGLE_SPEED_KP (0.15f)
#define CHASSISMOTOR_FOLLOW_ANGLE_SPEED_KI (0.f)
#define CHASSISMOTOR_FOLLOW_ANGLE_SPEED_KD (0.f)
#define CHASSISMOTOR_FOLLOW_ANGLE_SPEED_UIOUTMAX (0.3f)
#define CHASSISMOTOR_FOLLOW_ANGLE_SPEED_OUTMAX (1.0f)


#define CHASSISMOTOR_FOLLOW_LOCATION_KP (10.0f)
#define CHASSISMOTOR_FOLLOW_LOCATION_KI (0.f)
#define CHASSISMOTOR_FOLLOW_LOCATION_KD (15.0f)
#define CHASSISMOTOR_FOLLOW_LOCATION_UIOUTMAX (0.3f)
#define CHASSISMOTOR_FOLLOW_LOCATION_OUTMAX (1.0f)

#define CHASSIS_MODE_FREE (0)
#define CHASSIS_MODE_LOCK (2)
#define CHASSIS_MODE_GYRO (1)
#define CHASSIS_MODE_FOLLOW (3)
#define CHASSIS_MODE_AUTOSHOOT (4)

#define RAMP_DEC_SPIN (0.0012f)
#define RAMP_INC_SPIN (0.0003f)

#define FOLLOW_TOLERANT (0.005f)


#define GYRO_POWER_GAP (25.0f)
#define GYRO_OFFSET (0.03f)

#if NO_HERO == 0          // 旧英雄
#define GIMBAL_RESET_YAW_ENCODER (0.4170f) \
//(-0.0045f) // 新英雄
#elif NO_HERO == 1         // 新英雄
#define GIMBAL_RESET_YAW_ENCODER 0.06176f//0.02561f//-0.02032f//-0.4935//0.2673f

#elif NO_HERO == 2
#define GIMBAL_RESET_YAW_ENCODER 0.0065f
#endif
//飞坡点处的坡度为17，这里设置为10以避免误差
#define UPHILL (10)
#define FRONT (0.5)
#define REAR (1.5)

typedef struct
{
    uint8_t chassisMode;
    uint8_t lastMode;
    float speedX;
    float speedY;
    float lastspeedY;
    float speedSpin;    // 顺时针为正

    uint8_t lostRemote;
    uint8_t richestMotor;
    uint8_t superFlag;
    uint8_t resetFlag;
    uint8_t disCharging;
    uint8_t UphillFlag;
    uint8_t disCharging_Auto;
}ChassisControlDataStruct;

typedef enum {
    Lost = 0,
    Discharging = 1,
    Normal = 2
}Limit_Power_Mode;
/**
 * @brief 通过电机转速结算得到的底盘实际运动情况（主要用于键鼠控制的斜坡）
 * */
typedef struct
{
    float speedX;
    float speedY;
    float speedSpin;
}ChassisRealDataStruct;

//P = I^2R + HVI
typedef struct
{
    float nowPower;
    float limitPower;
    float paraR[5];         //功率环系数R
    float paraH[5];         //系数H
    float paraHinc;
    float paraHkp;
    float paraHinit;        //初始给的H值
    float paraHsumi;
    float k;
    float qA, qB, qC;       //qAqBqC是算乘数的中间变量，就是一元二次方程的a、b、c
    float UpHill_h;
    float UpHill_l;
    float Buffer;           //缓冲能量
}ChassisPowerControlDataStruct;

typedef struct {
    float RemoteK;
    float RemoteIncreaseK;
    //   float SpeedSum;
    float PowerLimitSpinDefault;
}ChassisSpeedDataStruct;

typedef struct
{
    struct{
        float	SetSpeed;              /*!< Speed setting value,
                                      This parameter can be a number between Min_Data = -1 and Max_Data = 1. */
        float	Speed;                 /*!< Speed values after normalization,
                                      This parameter can be a number between Min_Data = -1 and Max_Data = 1. */
    }Speed;

    struct{
        float	SetLocation;           /*!< Location setting value */
        float	Location;              /*!< Location values after normalization */
    }Location;

    PID PIDSpeed;                  /*!< Speed pid struct */
    PID PIDLocation;               /*!< Location pid struct */
    uint8_t PIDMode;               /*!< PID for speed or location */
}FollowControlDataStruct;

extern ChassisControlDataStruct  chassisControlData;
extern ChassisControlDataStruct  gimbalControlData;
extern FollowControlDataStruct followControlData;
extern ChassisPowerControlDataStruct chassisPowerController;
extern ChassisRealDataStruct gimbalRealData;
extern ChassisRealDataStruct chassisRealData;

void ChassisMotorInit(MotorStruct *motor);
void PowerControlInit();
void FollowControlInit(FollowControlDataStruct *control);

void PowerControlSpeed();
void SpeedLimitInit();
void ChassisMotorCalculate(void);
void ChassisMotorDataSend(void);
void ChassisMotorSetSpeed(void);
void ChassisControlDataUpdate(ChassisControlDataStruct *controlData);
void FollowControlDataUpdate(DM_MotorStruct * motor);
void ChassisFollowSetSpeed();
void ChassisGyroSetSpeed();
void ChassisSpeedFeedback();
int UpHillJudge();

void PidInit(MotorStruct* Motor,float kp,float ki,float kd);
float GetPowerNow();
float GetPowerBuffer();
float GetPowerLimit();
#endif //DRIVER_CHASSIS_H

/*电机示意
1           2
 1         2
  1       2
   1     2
   4     3
  4       3
 4         3
4           3
     前 ⬇
 */